//
// Created by cyq on 2022/12/9.
//

#include "task_communicate.h"
#include "usart.h"
#include "bsp_ina226.h"
#include "driver_chassis.h"
#include "queue.h"
#include "bsp_judge.h"
#include "protocol.h"
#include "string.h"
#include "task_lostcounter.h"

usart_param_struct judgement_usart;


void communicateTaskInit(void)
{
#if USE_JUDGE_OR_INA226
    //receiveJudgeHead();
	judgement_usart.huart = &huart6;
	judgement_usart.tx_finish_flag = 1;
	StartReception();

#else
    INA226_Init();
#endif
}

float readCurrent = 99.9f;
uint8_t tempBuffer[128] = {};
uint8_t tempBufferLen;
uint8_t popByte;
uint16_t tempcmdid;

void getJudgeData(void)
{
#if USE_JUDGE_OR_INA226
    uint8_t i;
	while (uxQueueMessagesWaiting(rxQueue) > 40)
	{
		//收SOF
		do
		{
			if (xQueueReceive(rxQueue, &popByte, 0) != pdPASS)
                return;
		} while (popByte != 0xa5);
		tempBuffer[0] = popByte;
        //一个一个接收队列中的信息
		for (i = 1; i < JUDGE_HEADER_LEN; i++)
		{
			xQueueReceive(rxQueue, &popByte, 0);
			tempBuffer[i] = popByte;
		}
		//对frame_header进行crc8验算（不成功就continue进行下一次数据解算）
		if (verify_crc8_check_sum(tempBuffer, JUDGE_HEADER_LEN) == 0)
			continue;
		for (i = JUDGE_HEADER_LEN; i < JUDGE_HEADER_AND_CMDID_LEN; i++)
		{
			xQueueReceive(rxQueue, &popByte, 0);
			tempBuffer[i] = popByte;
		}
		tempBufferLen = (tempBuffer[JUDGE_LEN_HIGH_INDEX] << 8) + tempBuffer[JUDGE_LEN_LOW_INDEX] + 2;
		tempcmdid = (tempBuffer[JUDGE_CMDID_HIGH_INDEX] << 8) + tempBuffer[JUDGE_CMDID_LOW_INDEX];

		for (i = JUDGE_HEADER_AND_CMDID_LEN; i < JUDGE_HEADER_AND_CMDID_LEN + tempBufferLen; i++)
		{
			xQueueReceive(rxQueue, &popByte, 0);
			tempBuffer[i] = popByte;
		}
		if (verify_crc16_check_sum(tempBuffer, JUDGE_HEADER_AND_CMDID_LEN + tempBufferLen) == 0)
			continue;


		judgeDataHandle(tempBuffer);
	}

#else
    GetPower();
    readCurrent = INA226_data.Power_Val;
    chassisPowerController.nowPower = INA226_data.Shunt_Current * INA226_data.Shunt_voltage / 1000000.0f;
    if(chassisPowerController.nowPower < 0) chassisPowerController.nowPower *= -1;
#endif
}

uint8_t Chassis_Gimbal_SendMessage[8];

void sendMessage2Gimbal(void)
{
    static uint8_t chooseId = 0;
    chooseId = (chooseId + 1) % 2;
    memset(Chassis_Gimbal_SendMessage, 0, sizeof(Chassis_Gimbal_SendMessage));
    if (chooseId == 0)
    {
        Chassis_Gimbal_SendMessage[0] =
                (judge_rece_mesg.game_robot_state_data.shooter_id1_42mm_cooling_limit & 0xFF00) >> 8;
        Chassis_Gimbal_SendMessage[1] = judge_rece_mesg.game_robot_state_data.shooter_id1_42mm_cooling_limit & 0x00FF;
        Chassis_Gimbal_SendMessage[2] =
                (judge_rece_mesg.power_heat_data.shooter_id1_42mm_cooling_heat & 0xFF00) >> 8;
        Chassis_Gimbal_SendMessage[3] = judge_rece_mesg.power_heat_data.shooter_id1_42mm_cooling_heat & 0x00FF;
        Chassis_Gimbal_SendMessage[4] = ((uint16_t)(judge_rece_mesg.shoot_data.bullet_speed * 1000) & 0xFF00) >> 8;
        Chassis_Gimbal_SendMessage[5] = (uint16_t)(judge_rece_mesg.shoot_data.bullet_speed * 1000) & 0x00FF;
        Chassis_Gimbal_SendMessage[6] =
                (16 & 0xFF00) >> 8;
        Chassis_Gimbal_SendMessage[7] = 16 & 0x00FF;
        if (judge_rece_mesg.game_robot_state_data.mains_power_shooter_output == 0)
        {
            Chassis_Gimbal_SendMessage[0] = 0;
            Chassis_Gimbal_SendMessage[1] = 0;
        }
        CAN2_Send_Msg(Chassis_Gimbal_SendMessage, 8, 0x1f1);
    }
    else
    {
        Chassis_Gimbal_SendMessage[0] = judge_rece_mesg.game_robot_state_data.robot_id;
        Chassis_Gimbal_SendMessage[1] = (judge_rece_mesg.game_robot_state_data.remain_HP & 0xFF00) >> 8;
        Chassis_Gimbal_SendMessage[2] = judge_rece_mesg.game_robot_state_data.remain_HP & 0x00FF;
        CAN2_Send_Msg(Chassis_Gimbal_SendMessage, 8, 0x1f2);
    }
}

void CommunicateTask(void *argument)
{
    /* USER CODE BEGIN CommunicateTask */
    uint8_t _count = 0;

    communicateTaskInit();
    /* Infinite loop */
    for(;;)
    {
        if ((GetErrorState() >> JUDGEMENT_LOST_COUNT) & 0x0001 && _count == 30) {
            HAL_UART_MspDeInit(&huart6);
            HAL_UART_MspInit(&huart6);

            communicateTaskInit();
        }
        _count = (_count + 1) % 50;

        getJudgeData();
        sendMessage2Gimbal();
        osDelay(1);


    }
    /* USER CODE END CommunicateTask */
}